import numpy as np
import tf.transformations as tfs
import cv2
import math

from geometry_msgs.msg import Pose

class CalculatePose:
    def __init__(self, yaml_file_path, coordination, depth, angle):
        self.path   = yaml_file_path
        self.center = coordination
        self.depth  = depth/1000
        self.angle  = angle
        self.external = None
        self.interal  = None
        self.distort  = None
        self.imgsize  = None

    def readparam(self, yfp):

        fs2 = cv2.FileStorage(yfp, cv2.FileStorage_READ)
        self.external = fs2.getNode('CameraExtrinsicMat').mat()
        self.interal  = fs2.getNode('CameraMat').mat()
        self.distort  = fs2.getNode('DistCoeff').mat()
        self.imgsize  = fs2.getNode('ImageSize').mat()
        fs2.release()

    def runcalculate(self):

        self.readparam(self.path)

        campoint   = np.ones((3,1),dtype=float)
        u0 = self.interal[0,2]
        v0 = self.interal[1,2]
        fx = self.interal[0,0]
        fy = self.interal[1,1]

        campoint[0,0] = ((self.center[1]-u0)/fx)*self.depth
        campoint[1,0] = ((self.center[0]-v0)/fy)*self.depth
        campoint[2,0] = self.depth

        ET = self.external[0:3,-1:]
        ER = self.external[0:3,0:3]
        
        basepoint = ER*campoint + ET

        ER1 = tfs.euler_matrix(self.angle,0,0)
        ER2 = np.linalg.inv(ER)
        ER3 = ER1*ER2
        orientation = tfs.quaternion_from_matrix(ER3)
        
        pose = Pose()

        pose.position.x = basepoint[0]
        pose.position.y = basepoint[1]
        pose.position.z = basepoint[2]
        pose.orientation.x = orientation[0]
        pose.orientation.y = orientation[1]
        pose.orientation.z = orientation[2] 
        pose.orientation.w = orientation[3]


        # return basepoint,orientation
        return pose










 